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Residual Form Method 
to compute and p u 



Direct Form &Jethod 
to compute q and u 



1. Compute the First Kinematics dje, 
and" the kinematic residual P 0 (k) 

2 . Generate f(k) , the spatial load 
balance for each body 

3. Compute dynamic residual p u (k) 



1. Compute q using joint 
specific routines 

2 . Perform First Kinematics C<xic^ 
with ii~0 

3 . Generate residuals P u and 
negate p u = -p B 

4. Perform Second Kinematics CJusu 

5. Compute fusing Forward 
Dynamics * 




Fig. 3 
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1. Compute the analytic Jacobians of the kinematics 
routines : 



d{Wu) 
dq 



and J qx =W 



2. Compute Z = -M' 1 p u (q>U 9 0) using the Direct Method 

3. Compute the analytic Jacobians of the dynamics 
Residual routine: 

dq du 

4. Backsolve for the analytic Jacobian of the dynamics 
routine using results for z from the Second 
Kinematics step: 

" a? dq 

uu ~8u~ du 



Fig. 6 
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